/*
	tello石景山飞行试验速度控制算法
	算法采用P控制进行定点跟踪，采用人工势场法进行实时避障与机间避碰
	算法改进为用类的形式进行实例储存
*/
#include "tello_control/Tello.h"

int main(int argc, char **argv)
{
	int i, j;
	int num = tello_num;
	double r1 = 1.0, r2 = 1.5, r3 = 0.6, w = 0.2616;//定义偏移半径与角速度
	double vmax = 1.0;//最大速度
	//t1,t2代表旋转时间的两个时间点，t3代表飞机跟车离开的时间点,t4,t5分别代表节点损伤与节点恢复的时间点
	double t1 = 130, t2 = 105, t3 = 130, t4 = 20, t5 =30;
	double roll_time = 24;//编队旋转的总时长
	Posture loss_pos;//节点丢失的临时位置
	int loss_flag = 0;
	ros::init(argc, argv, "tello_vel_v3");
	ros::NodeHandle nh("");

	geometry_msgs::PoseStamped uav_center;//编队中心从外部算法接收
	//用堆的方式存储Tello类，这样内存会更加安全
	std::vector<Tello*> uavs;
	std::vector<Ugv*> ugvs;
	// uavs 中存储了多个 Tello 对象的指针，这样可以方便地管理和访问多个对象。
	for(i = 0;i < tello_num;i ++)
		uavs.push_back(new Tello());
	//从地面站订阅tello的当前位置（mocap - xgc - tello）
	for(i = 0;i < tello_num;i ++)//定义uavs_ros通信相关的变量
	{
		uavs[i]->setID(i + 1);//先置入ID信息,也可以单独置入
		uavs[i]->initsubscriber(nh);
		uavs[i]->initpublisher(nh);
	}
	//实例化小车对象
	for(i = 0;i < ugvs_num;i ++)
	{
		// cout<<"实例化小车完成";
		ugvs.push_back(new Ugv());
	}
	//从地面站订阅tello的当前位置（mocap - 
	//订阅小车位置
	for(i = 0;i < ugvs_num;i ++)
	{
		ugvs[i]->setID(i + 5);
		ugvs[i]->initsubscriber(nh);
		// cout<<"set小车ID完成,ID为:"<<ugvs[i]->getID()<<endl;
	}

	//发布当前的中心点位置信息(指令中心位置)
	ros::Publisher center_pub = nh.advertise<geometry_msgs::PoseStamped>("/tello_center", 10);
	//订阅障碍物位置
	ros::Subscriber obs_sub = nh.subscribe("obstacles",10,cb_obstacle);

	ros::Subscriber pos_cmd = nh.subscribe("pos_cmd", 10, cb_center);
	ros::Subscriber cmd_sub = nh.subscribe("/command", 10, cb_command);//订阅控制命令信息

	ros::Rate loop_rate(50);

	while (ros::ok())
	{
		double hx, hy, vx, vy;
		double t = ros::Time::now().toSec() - tstart; //初始化计时器
		uav_center.pose.position.x = 0, uav_center.pose.position.y = 0;
		for(i = 0;i < num;i ++)
		{
			uav_center.pose.position.x += uavs[i]->pos.x/num;//中心点计算代码
			uav_center.pose.position.y += uavs[i]->pos.y/num;
			// cout<<"i:"<<num<<"posx:"<<uavs[i]->pos.x; 
		}
		// cout<<"小车位置:"<<ugvs[0]->pos.x<<endl;
		center_pub.publish(uav_center);//计算四架飞机的中心并发布，检测飞行是否稳定
		//检测绕圈时间
		if(abs(center.rolling_flag - 1)<0.01)
		{
			cout<<"检测到绕圈指令"<<endl;
			t1 = center.rtime_start;
			t2 = t1 + roll_time;
			t3 = center.rtime_end;
			cout<<"绕圈开始的时间为:"<<t1<<"结束的时间为"<<t2<<endl;
		}
		// if(abs(center.arrived - 1)<0.01)
		// {
		// 	cout<<"检测到任务即将结束"<<endl;
		// 	t3 = center.rtime_end;
		// }
		if (command == 0) // 编队初始化
		{
			for (i = 0; i < num; i++)
			{
				// 编队设置
				hx = r1 * cos(2 * i * PI / num), hy = r1 * sin(2 * i * PI / num);
				vx = 0, vy = 0;
				uavs[i]->setform(hx, hy, vx, vy);
				uavs[i]->vepul(uavs);//判断是否需要避障
				// 计算输入
				uavs[i]->vel_control1(center, vmax);
			}
		}
		// if (command == 1) // 起飞
		// {
		// 	// cout<<"起飞"<<endl;
		// 	std_msgs::Empty takeoff_cmd;
		// 	for(i = 0;i < num;i ++)
		// 	{
		// 		uavs[i]->takeoff_pub.publish(takeoff_cmd);
		// 	}
		// }
		if (command == 2)
		{
			//外部的位置指令开始移动，四机跟随中心点开始运动
			// cout<<"编队正常进行中"<<endl;
			for(i = 0; i < tello_num; i++)
					uavs[i]->vepul(uavs);//判断是否需要避障
			if (t > t4 && t < t5)
			{ // 4号机停止
				cout << "4号机节点损伤,编队变为3机" << endl;
				uavs[3]->setform(0,0,0,0);
				if(loss_flag == 0)
					loss_pos = uavs[3]->pos, loss_flag = 1;
				uavs[3]->vel_control1(loss_pos,vmax);
				// 变更为3机编队
				num = 3;
			}
			if (abs(t - t5) < 0.02)
			{
				cout << "4号机节点恢复,编队变为4机" << endl;
				num = 4; // 变更为四机编队	
			}
			if (t <= t1)
			{
				for(i = 0;i < num;i ++)
				{
					//编队设置
					hx = r1*cos(2*i*PI/num), hy = r1*sin(2*i*PI/num);
					vx = 0, vy = 0;
					uavs[i]->setform(hx,hy,vx,vy);
					//计算输入
					uavs[i]->vel_control1(center,vmax);
				}
			}
			//四机到达指定位置，开始进行盘旋检测(t1,t2根据外部轨迹信息进行确定)
			else if (t > t1 && t <= t2)
			{
				for(i = 0;i < num;i ++)
				{
					hx = r2 * cos(w * (t - t1) + 2 * i * PI / num), hy = r2 * sin(w * (t - t1) + 2 * i * PI / num);
					vx = -r2 * w * sin(w * (t - t1) + 2 * i * PI / num), vy = r2 * w * cos(w * (t - t1) + 2 * i * PI / num);
					uavs[i]->setform(hx,hy,vx,vy);
					uavs[i]->vepul(uavs);//判断是否需要避障
					//计算输入
					uavs[i]->vel_control1(center,vmax);
				}
			}
			else if (t > t2 && t <= t3)
			{
				for (i = 0; i < num; i++)
				{
					hx = r1*cos(w * (t2 - t1) + 2*i*PI/num), hy = r1*sin(w * (t2 - t1) + 2*i*PI/num);
					vx = 0, vy = 0;
					uavs[i]->setform(hx, hy, vx, vy);
					// 计算输入
					uavs[i]->vepul(uavs);//判断是否需要避障
					uavs[i]->vel_control1(center,vmax);
				}
			}
			//四机完成检测任务，跟车离开
			else
			{
				for(i = 0;i < num;i ++)
				{
					hx = r1*cos(w * (t2 - t1) + 2*i*PI/num), hy = r1*sin(w * (t2 - t1) + 2*i*PI/num);
					vx = 0, vy = 0;
					uavs[i]->setform(hx,hy,vx,vy);
					//判断是否需要避障
					uavs[i]->vepul(uavs);
					//计算输入
					uavs[i]->vel_control1(center,vmax);
				}
			}
		}
		if (command == 3) // 指令速度给0
		{
			for(i=0;i<num;i++)
			{
				uavs[i]->cmd_vel.linear.x = 0;
				uavs[i]->cmd_vel.linear.y = 0;
				uavs[i]->vel_pub.publish(uavs[i]->cmd_vel);
			}
		}
		// if (command == 9) // 急停降落
		// 	{
		// 		std_msgs::Empty land_cmd;
		// 		for (i = 0; i < num; i++)
		// 			uavs[i]->land_pub.publish(land_cmd);
		// 	}
		ros::spinOnce();
		loop_rate.sleep();
	}
	// 清空向量
	uavs.clear();
	ugvs.clear();
	return 0;
}